#include "Debug.h"
#include "AngleFlattener.h"


AngleFlattener::AngleFlattener(MItMeshEdge* edgeIt, MItMeshPolygon* faceIt, MItMeshVertex* vertsIt, vector<int> seamVerts, vector<int> seamEdges)
{
	as = new AngleSpace(edgeIt, faceIt, vertsIt, seamVerts, seamEdges);
}

AngleFlattener::~AngleFlattener()
{
   delete as;
}

int iterationCounter = 0;

double constraintPlanarValidity(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
	AngleSpace* as = reinterpret_cast<AngleSpace*>(data);
	//as->updateAngles(x);
	double pv = as->planarValidity();
	grad.resize(x.size());
	as->calcGradPlanar(grad);
	//if(iterationCounter%1000000000)	
	//	cout << "PV: " << pv << endl;
    return pv;
}

double constraintWheelValidity(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
	AngleSpace* as = reinterpret_cast<AngleSpace*>(data);
	//as->updateAngles(x);
	double wv = as->wheelValidity();	
	grad.resize(x.size());
	as->calcGradReconstruction(grad);
	//if(iterationCounter%1000000000)	
	//	cout << "WV: " << wv << endl;
	return wv;
}

double constraintTriangleValidity(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
	AngleSpace* as = reinterpret_cast<AngleSpace*>(data);
	double tv = as->triangleValidity();
	grad.resize(x.size());
	as->calcGradTriangle(grad);

	//if(iterationCounter%1000000000)	
	//	cout << "TV: " << tv << endl;
	
	return tv;
}

double objectiveFunction(const vector<double> &x, vector<double> &grad, void *data)
{
	iterationCounter++;
	AngleSpace* as = reinterpret_cast<AngleSpace*>(data);
	as->updateAngles(x);
	double f = as->angularDistortion();

	grad.resize(x.size());
	as->calcGradObjective(grad);
	
	//if(iterationCounter%100000000000)
	//{
	//	cout << "F: " << f << endl 
	//	cout << "Iter: " << iterationCounter << endl << endl;
	//}

	return f;
}

AngleSpace* AngleFlattener::flattenAngleSpace(int type, int time)
{
	vector<double>* initialAngles = as->getAngles();
	vector<double> x(*initialAngles);
	delete initialAngles;

	double minf;
	int size = (int)x.size();

	nlopt::opt* localOpt;
	cout << "Time: " << time << " Type:" << type << endl;
	if(type == 0)
	{
		localOpt = new nlopt::opt(nlopt::LN_PRAXIS, size);
		cout << "Praxis Optmisation" << endl;
	}
	else
	{
		localOpt = new nlopt::opt(nlopt::LN_NELDERMEAD, size);
		cout << "Nelder-Mead Optmisation" << endl;
	}

	nlopt::opt opt(nlopt::LN_AUGLAG, size);
	opt.set_min_objective(objectiveFunction, as);
	opt.add_equality_constraint(constraintPlanarValidity, as, 0.0001);
	opt.add_equality_constraint(constraintWheelValidity, as, 0.0001);
	opt.add_equality_constraint(constraintTriangleValidity, as,0.0001);

	opt.set_lower_bounds(0);
	opt.set_upper_bounds(180);
	
	localOpt->set_xtol_rel(0.1);
	opt.set_local_optimizer(*localOpt);
	opt.set_xtol_rel(0.1);
	
	opt.set_maxtime(time);
		
	cout << "Optimising" << endl;
	
	nlopt::result result = opt.optimize(x, minf);	

	cout << "found minimum angles at " << minf << endl << "found minimum after " <<  iterationCounter << " evaluations" << endl << "Minimum Angles: " << endl;
	cout << "Triangle: " << as->triangleValidity() << endl;
	cout << "Planarity: " << as->planarValidity() << endl;
	cout << "Reconstructablity: " << as->wheelValidity() << endl;

	//as->printAngleSpace();
	delete localOpt;
	return as;
}